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ABSTRACT 


Recent commercial developments in small, low cost real-time kinematic GPS sensors en¬ 
able position realization for light, mobile platforms with centimeter-level accuracy. With 
a high degree of positional confidence, we explore the feasibility of close-proximity op¬ 
erations of cooperative autonomous agents in an outdoor, GPS-enabled environment. A 
computer-simulated hookian spring force is used to control a “swarm” of robotic agents, 
a technique called artificial physics. The computer model applies a proportional spring 
constant based off position information, and the resultant force is applied to each agent re¬ 
spectively. We validate the model by comparing it to analytic solutions, then further refine 
the model by comparing it to field testing data. With an accurate model of the system, user- 
defined tasks are tested in simulations and the same algorithm then controls the behavior 
of the robotic swarm in an outdoor environment. 
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Executive Summary 


Previous research at NPS explored the use of physics-based computer algorithms to con¬ 
trol the behavior of a multi-agent unmanned aerial vehicle (UAV) system in a laboratory 
setting. This paper explores a variation of that technique that can be used in outdoor, GPS- 
enabled environments. Analytic predictions inform computer simulations, which support 
live field experimentation. Data from the real-world experimentation is then analyzed and 
the computer simulation is refined to provide a more accurate model of the actual system. 

The intent of this research is developing a multiple autonomous agent system in an outdoor, 
GPS environment where the agents can operate in close-proximity to each other. To do 
this, a computer-simulated virtual spring connects each of the agents in the system. By 
adjusting the “unstretched” length of the spring, the individual agents move until all the 
virtual spring forces experienced by that agent are in equilibrium. The result is a system 
that self-organizes into a formation centered around a mobile base station. 

This type of self-organizing formation behavior can be thought of as a swarm. Robotic 
swarm behavior relieves the burden of an operator directly controlling the actions of each 
individual agent, while still accomplishing a desired outcome to be performed by the sys¬ 
tem. This allows the operator to focus on high-level tasking and lets the swarm behavior 
algorithm determine how best to accomplish that tasking. 

To accomplish a model of this behavior, a computer algorithm calculates the distance be¬ 
tween one agent and every other agent in the system separately, and applies a proportional 
spring constant to the difference. This creates an artificial “spring force” between one agent 
and all the other agents in the system separately. The corresponding forces are added as a 
vector sum, and a total resultant force is designated to the corresponding agent. The force is 
converted into an acceleration, the acceleration is integrated into a velocity, and the velocity 
is integrated into a position. This is repeated for every agent in the system. The agents in 
the simulation move to their new position for each time step of the loop, and the new posi¬ 
tions of each of the agents are used for the next iteration. The process is repeated until the 
net force on each agent is zero, the agents stop moving, and the system is at equilibrium. 

Performing this in an outdoor, GPS environment, the same computer-simulated spring force 


XV 




algorithm is used. However, real-world position and orientation information of the indi¬ 
vidual agents are given as inputs to the model. This research utilizes real-time Kinemat¬ 
ics (RTK) GPS to obtain accurate position information and onboard inertial measurement 
units (IMUs) for orientation information of the agent. After calculating the net force and 
the subsequent desired velocity of each individual agent, a velocity command is sent to 
each respective agent, and its updated RTK GPS position is used in the next iteration of the 
spring force algorithm. 
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CHAPTER 1: 
Introduction 


1.1 Motivation for Swarm Research 

The Department of Defense (DoD) has identified military applications of autonomous sys¬ 
tems as a major force multiplier. Extensive efforts in the fielding of these systems make 
them a viable option for front-line commanders. From 2005 to 2011, there was a 1,200% 
increase in Unmanned Aerial Vehicle (UAV) combat air patrols and more flight hours con¬ 
ducted by UAVs than by manned strike aircraft [1]. The funding has followed, although 
at a slower pace, with an increase in DOD spending on unmanned system from $284M in 
FY2000 to $5.6B in FY2013 [2]. 

Research and Development in the area of autonomous systems is also enthusiastically 
gaining momentum. The Office of Naval Research recently demonstrated the capability 
of unmanned, self-guided boats to escort a vessel and swarm a target [3]. The Defense 
Advance Research Project Agency launched its Anti-submarine Warfare Continuous Trail 
Unmanned Vessel demonstrator in January 2016 [4]. Unmanned systems are gaining pop¬ 
ularity not only in the DOD, but in commercial applications as well. Amazon began testing 
delivery orders via multi-rotor UAVs in April 2015 [5] and Google had logged over 1.5 
million miles by self-driven cars as of April 2016 [6]. 

The present-day commonality of autonomous systems leads to a pressing problem for the 
Navy. How will unmanned systems affect the way future wars are fought at sea? The Naval 
strategy for the past 75 years remains unchanged, centering on power projection at sea with 
an aircraft carrier. The carrier battlegroup commander has significant advantage over the 
commander of a conventional surface ship because embarked aircraft offer range superior¬ 
ity over the enemy. However, as technology advances, leaders must focus their efforts on 
what will give the greatest maritime advantage in the future. 

Unmanned Systems can offer a number of advantages over traditional manned systems, 
such as decreased risk to personnel, long dwell time, no life support systems required, and. 
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with a satellite data link and pre-staged assets, can offer an immediate physical presence 
anywhere in the world without a constantly deployed fleet. The computer and technology 
industry advances have made owning and operating unmanned systems much more acces¬ 
sible than they were even five years ago. However, inexpensive unmanned systems must 
be coordinated en masse to “swarm” and overwhelm an enemy’s defenses in order to have 
the same effect as larger, more expensive and complex weapons. 


Although the low material cost and easy access to online open source content lowers the 
barrier for entry into the field of robotics, the integration of various platforms and sensors 
to operate autonomously is still a challenging problem. The primary sensor involved in this 
research is real-time kinematics GPS, where calculation of a GPS-carrier signal differential 
produces a centimeter-level accurate position vector between modules [7]. 

In addition to reliable and accurate sensors, effective and efficient control of unmanned 
system is essential. Control algorithms to accomplish this purpose are often implemented 
to relieve the requirement of a human operator to directly control the agent(s). For well de¬ 
fined and predictable tasks, predetermined actions are set by the controlling algorithm and 
are executed by the system, called a “brute force” style of control. For instance, a welding 
robot arm on a car assembly line exhibits this style of control. The welding spots do not 
change from car to car, so a programmer ensures the robot makes the same movement each 
time a new car comes through the assembly line. 

However, this “brute force" approach for robotic systems in ill defined and uncontrolled 
environments is a daunting task; predicting the multitude of potential situations that a sys¬ 
tem may experience is nearly impossible. The potential number of situations also increases 
as more agents are added to the system. A different approach is developed by looking at 
natural physical phenomena. 

Natural laws of physics govern interactions between particles both big and small in the 
universe. Complex structures arise out of particles obeying simple physical forces, such 
as galaxies forming due to gravity and DNA molecules arising from interactions of the 
electromagnetic force. By programming robotic agents to react to simulated forces, similar 
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types of behavior can be realized. 


1.2 Previous Work 

Directing physical agents toward a location with the lowest computer-simulated potential 
is a concept called potential field method (PFM) first proposed by Hogan [8] to navigate 
a manipulator arm around obstacles in a fixed environment. Problems with this approach 
are explored by Koren and Borenstein [9]. One major problem is that while the individual 
agents are moving toward the lowest potential in a system, an agent could get caught in a 
local potential minimum that may not be the lowest possible potential of the system. If the 
agent does not have enough energy to get out of the local minimum, the agent will never be 
able to reach the global minimum, and the desired behavior of the system is never realized. 
One possible solution to this problem is explored in Section 2.3. 

One of the frameworks utilizing an PFM approach has been developed for distributed 
control of swarms of robots by Spears et al. [10] called physicomimetics. Using natural 
physical laws as a guide, physicomimetics is a method whereby a computer imposed inter¬ 
action between agents form the basis for the movement of each individual agent. Instead 
of pre-determining the entire potential field of the given environment, physicomimetics in¬ 
troduces algorithm based virtual forces that are imposed on the agents in a system. 

This approach is also called artificial physics (AP) because although the interactions and 
applied forces are all virtual, the agents behave just as particles under the influence of phys¬ 
ical laws in the real-world. Apker and Potter in [11] used the laws governing the states of 
matter, solids, liquids and gases, to have the robotic agents execute different types of be¬ 
havior for their system. 

The key issue is choosing the sensor information that the computer algorithm will use to 
determine the swarm behavior. Ma’sum et al. [12] used three quadrotor aircraft in a similar 
network architecture to the one presented in this study. However, they used computer vi¬ 
sion for their main tracking sensor. Difficulties in using computer vision and LIDAR-based 
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sensor information to determine robot localization are presented in Chapter 3. 

Numerous research groups use a high speed, near-infrared camera system, called Vicon 
motion capture [13], to track the motion of reflective markers on an object with mil¬ 
limeter accuracy. The result is a 3D computer generated object with position and ori¬ 
entation information in the Vicon coverage area. Kushleyev et al. at the GRASP Lab 
at University of Pennsylvania [14] utilize this approach (with some fantastic video at 
http://youtube.com/user/TheDmel/videos). While amazing, it requires significant amount 
of installed infrastructure and cannot easily be replicated in an outdoor environment. 

Another very common position realization technique is GPS, and Alvissalim et al. [15] 
place a GPS receiver on multiple quadrotors and use the GPS coordinates to self-deploy 
and extend the coverage of a Wi-Fi network. The approach is similar, but since the indi¬ 
vidual quadrotors are nominally separated by large distances to affect an extended Wi-Fi 
network, each quadrotor’s location does not need to be extremely accurate since the chance 
of collision is small. 

Given the potential benefits that robotics can offer to the maritime domain, this thesis 
explores the use of various sensors and platforms in an artificial physics environment to 
perform a task requiring a high level of precision. To demonstrate the capability of the 
system, the scenario will be having multiple UAVs take off and follow in close-proximity 
to an Unmanned Surface Vessel (USV). 
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CHAPTER 2: 

Artificial Physics Approach 


The approach used in this study is a system of agents (agents) experiencing attractive and 
repulsive forces to maintain a certain desired distance from nearby agents. The end state of 
the system will be one where each agent rests in a potential well, which will be determined 
by the combination of attractive and repulsive forces felt by a single agent. This simple 
concept will allow for systematic formation of a collection of mobile robots. 

The artificial physics environment is set up so that the separate agents experience a “force” 
of attraction or repulsion depending on the requirement of the task. This “force” is imposed 
by a computer algorithm, which causes a physical reaction from the platform. For example, 
if the operator wanted a UAV to fly toward an object, (s)he would set up the algorithm so 
that the UAV “felt” an attractive force in the direction of the object. 


2.1 A Physics Foundation 

The basis for the desired actions of the individual agents is firmly rooted in well-defined 
classical physics. Summoning Newton’s second law of motion, the force exerted on a parti¬ 
cle will accelerate the particle (F = ma). The agents start in random locations in a specified 
2D grid. For our purposes, we will treat each agent as a particle point mass with m = 1 kg. 

The previous research at NFS [16] utilized a gravity-like force expressed as 


F 


mim2. 
= G——r 


( 2 . 1 ) 


where mi and m 2 are the masses of the two agents and r is the distance between them. In 
the previous research, the G constant was a user-controlled variable that could be adjusted 
through a keypad on a handheld device. Since gravity is an attractive-only force, the al¬ 
gorithm had a “switch over” distance, where the force would change from being attractive 
to being repulsive to prevent the agents from colliding with each other. This 2 potential 
is not ideal, since the “force” felt by the agent increases as the agent gets closer to the 
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desired final location at the bottom of the potential well and decreases the farther away it 
is. In practice, that means the agent will have sharper and more drastic movements when 
it is near a neighboring agent’s location, whereas the agent will have very faint movements 
when it is very far away. 

A better solution for such a system could be modeled as a mass-spring system. In this 
case, an agent that is very far from its optimum spacing will have a very strong force push¬ 
ing or pulling it, and when the agent is near its final desired location, the force will be 
much smaller. The artificial force on an agent from another agent then becomes a simple 
application of Hooke’s law 


Fi^j = -k(xij - xo) (2.2) 

where k is the spring constant that controls how strong the force of attraction or repulsion 
acts, Xij is the distance between the ith agent and the jth agent, and xq is an optimum 
spacing between the agents (Figure 2.1). 
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Figure 2.1: 


Diagram of Artificial Spring Force on an Agent. 
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Consider only the x components in Cartesian coordinates of a system of agents. By sum¬ 
ming up the X forces acting on a single agent from all other agents, we get the total net 
force in the x direction that is acting on a particular agent 

N 

Pi,net = ^ -k{Xij - Xo)x (2.3) 

./ = 1 

where N is the total number of agents in the system. 

We are modeling this as linear system and the principle of superposition holds. That allows 
us to predict that the overall force on the agent from two or more forces is the sum of the 
result that would have been caused by each force individually. In addition, once the force 
is known, we can analyze of dynamics of a single agent as if it were a single mass-spring 
system by itself, and the total dynamics of the system as a whole is the aggregate sum. The 
outcome of the computer simulation will let us know if our model accurately portrays the 
analytic solution. 

The oscillatory nature of the spring force invites us to introduce viscous damping term 
in order to reach a stable equilibrium more expeditiously 

^ dx: 

Pivotal = Pifiet - yS —i (2.4) 

dt 

where /3 is the damping coefficient. 

The Ftotai is calculated for every agent in the swarm, taking care not to calculate a spring 
force exerted by the agent on itself! 


2.2 Numerical Integration 

Once the total force in the x direction on a agent is known, we can then determine the x 
behavior of that agent based off of classical mechanics. 
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A common technique used to calculate trajectories of particles in molecular dynamics com¬ 
puter simulations is the Verlet Velocity algorithm. This numerical method for integrating 
differential equations calculates the velocity and position of the particle at the same value 
of the time variable. 


a(t + dt) = (2.5) 

m 

v(t + dt) = v(t) + -(d(t) + d(t + dt))dt (2.6) 

1 , 

x{t + dt) = x{t) + v(t)dt + -d(t)dt (2.7) 

The same procedure is done for the y component in Cartesian coordinates, giving a 2D 
solution for the system for given values of k and yS. 


2.3 Analytic Prediction 

To determine values for k and /3 that give reasonable solutions for the scenario, we begin 
by looking for an analytic solution. Since this is a multi-body problem, an exact analytic 
solution (especially one with arbitrary number of agents with arbitrary mass) can be dif¬ 
ficult, if not impossible, to obtain. We start by making the approximation that the system 
as a whole can be described by a single agent exhibiting damped harmonic motion, and 
compare those results to the results from a computer simulation. 

Assuming the net force on an single agent is a simple spring. Equation 2.4 now becomes 


Ptotal ~ p! 


sping 


^ dx ^ 

F’damping ~ ~k(^X — Xo)x — ~ • 


Or, in the form of a differential equation. 


( 2 . 8 ) 


d^x dx 

+ /3— + kx = 0, 


dP 


dt 


(2.9) 


this homogeneous second-order differential equation has solutions of the form 
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X = e 


( 2 . 10 ) 


At 


The auxiliary equation for A is then 


mA^ + /3A + k = 0, (2.11) 

and the roots for the quadratic equation are 


A = 


-jS + J3^ - 4-mk 

2m 


( 2 . 12 ) 


The discriminant tells the type of system that we would expect to see. If it is positive, the 
system is overdamped. If negative, the system is underdamped. If the discriminant is 0, 
then the system is critically damped. 


The values for k and jS are completely user-defined, since there is no “actual” spring, it 
is simply a virtual force. For example, a spring constant k = 2 N/m (and m = 1 kg) would 
mean for a critically damped system 


f-4(l)(2)=0 (2.13) 

jS would have to be 2.82 kg/s to make the system critically damped. A smaller value would 
make the system underdamped, and a higher value would make the system overdamped. 
The same procedure can be done to find k if given a specified /3. 


2.4 Simulation Results 

The MATLAB simulation incorporated an arbitrary number of ten agents, each with a mass 
= 1 kg. This is easily scalable to include more or heavier agents, but to investigate the ef¬ 
fects of the spring constant and the damping coefficient, the number of agents was kept 
constant throughout. 
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The agents are started in random loeations on a 100x100 meter grid (Figure 2.2). 



100 


X Distance (m) 

Figure 2.2: Plot Agents at Time=0 Seconds. 


From this position, eaeh agent exerts an attraetive foree on eaeh of the other agents as 
diseussed before. 

The attraetive foree draws the agents eloser together (Figure 2.3). The speed with whieh 
the agents move is dependent on two faetors, the spring eonstant, k, and the damping 
eoeffieient /3. We will diseuss these more in depth later. 

A eonsequenee of Hooke’s law in this applieation is the agents will push apart from eaeh 
other if they are eloser than the optimal spaeing distanee Xo- Eaeh of the agents is now 
experieneing both an attraetive foree and a repulsive foree. This eauses the agents to self- 
arrange into a formation in whieh the spaeing between the agents is equal for all agents 
(Figure 2.4). 

Two faetors, the spring eonstant k and the damping eoeffieient /3, influenee the agents’ 
ability to arrange into an optimum formation. Sinee both of these faetors are at work in 
the simulation, our strategy was to hold one faetor eonstant while varying the other, and 
then switeh. To that end, holding the spring eonstant at a value of k = 2N/m, we vary the 
damping eoeffieient (5 (Figure 2.5). 

As shown in Figure 2.5, a value of jS = 1 kg/s gives an underdamped solution, where 
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Figure 2.3: Plot Agents at Time=0.5 Seconds. 
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Figure 2.4: Plot Agents at Time=l Second. 


the agents oseillate around the optimized distanee and do not reaeh equilibrium (at least 
not within a reasonable amount of time!). A value of = 10 kg/s gives an overdamped 
solution, where the agents take a longer time to reaeh the optimum spaeing. The value of 
jS = 4 kg/s gives a slightly less than eritieally damped solution, whieh allows us to see a 
slight oseillation to the agent, which gives confirmation that the agent is actually at opti¬ 
mum spacing. 

Next, the damping coefficient yS is held constant and the effect of varying the spring con¬ 
stant k is analyzed (Figure 2.6). 
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Figure 2.5: Plot of Average agent Distance against Time with Spring Con¬ 
stant k=2 N/m. 



Figure 2.6: Plot of Average agent Distance against Time with Damping 
Coeff. /3=4 kg/s. 


The spring constant is a measure of how strong the force of attraction or repulsion is 
between any two agents. Again, we can see and underdamped solution {k = 5N/m), an 
overdamped solution {k = 0.5N/m) and a slightly less than critically damped solution 
{k = 2N/m). 


It is observed that in order for the system to attain equilibrium with the desired spacing 
between each of the agents, the system has to have a high enough k to overcome viscous 
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damping, but low enough to settle out in a desired amount of time. The system must also 
have a high enough damping coefficient jS so that the system does not oscillate continu¬ 
ously, but low enough that the agents can actually move. 

For the objectives of this simulation, values of m = 1kg, k = 2N/m and yS = 2.8kg/s 
prove sufficient in organizing agents into an acceptable formation in a reasonable amount 
of time. 

This corresponds almost exactly with our analytic prediction, proving that damped har¬ 
monic motion solution is a valid assumption for the computer model. 


2.4.1 Problem with PFM 

As stated previously, while the agents are moving according to the forces acting on them, 
a agent could get caught in a potential well that may not be the lowest possible potential of 
the system. Without sufficient energy to escape, the agent will settle and reach equilibrium 
in the local minimum potential well. The physical result is that the agents in the system will 
not be all equally spaced, and the intent of a symmetric formation will never be realized. 

With randomly placed agents, this is a very distinct possibility. To simplify the problem, 
the initial conditions are imposed on the system so the probability for local minima is very 
low. The initial conditions are such that the agents start symmetrical to each other with the 
base station in the middle. This ensures that the minima reached for each of the agents at 
equilibrium is the global minimum for the system. 

For systems where the initial conditions cannot be set, the methods outlined in this pa¬ 
per can still be utilized. Many simulations can be run in order to find which simulation 
has the lowest possible potential, and then compare that with the system in question. If the 
system in question is not at the lowest possible potential, then one of the agents is most 
likely “stuck” in a local minimum potential well. One could then perturb the system in an 
effort to get the agent out of the well. 
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2.5 Rotational Compensation 

The algorithm gives the desired x-y movements that each agent makes in the fixed refer¬ 
ence frame of the 2-D plot. However, real-world agents (especially quadrotors) can rotate 
independently of the fixed reference frame. To account for this difference in heading di¬ 
rection, a rotational transformation matrix is required. If an agent is at a certain location, 
the artificial physics algorithm determines that the agent needs to move in a certain x-y 
direction. Given the current angle of the agent’s heading relative to a predetermined initial 
heading (i.e.. North), the rotation transformation matrix will convert the required direction 
into the agent’s reference frame, so that the agent’s left-right/forward-backward motors can 
then produce the overall desired action. 


The 2-D rotation matrix is as follows: 


R(0) = 


cos{6) -sin{6) 
sin{6) cos{9) 


Given a desired x and y velocity, Vx and 


V'x 


cos{6) -sin{6) 

Vx 



sin{6) cos{9) 



where 6 is the angle of the agent heading with respect to true north, and a' and v'^ are the 
actual velocity commands that are sent to the agent to accomplish the desired behavior in 
the fixed reference frame. 
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CHAPTER 3: 

Sensor Selection and Hardware Implementation 


3.1 Requirements and Available Systems 

To replicate the laboratory experimentation in an outdoor environment, a sensor system is 
required to replicate the position and orientation information that was previously provided 
by the installed Vicon system. The sensor system must be small and light enough to allow 
the agent to take off and maneuver, yet accurate enough for the applications such as land¬ 
ing on a small USV platform. We will first discuss the issue of position realization in an 
uncontrolled, infrastructure-free environment. 

There are a few ways to resolve location information in real-world environments. A com¬ 
mon tactic is to use a camera with computer vision software. Knowing what the camera 
expects to see, a programmer can set up an algorithm to determine the agent’s location 
based on visual cues. This is, of course, dependent on line of sight, lighting conditions, 
visual angle considerations, as well as computing power, since computer vision algorithms 
can be quite processor intensive depending on the requirements. 

Another favored technique used in location determination is wave reflection as seen in 
SONAR/RADAR/LIDAR. Using a laser, microwave or ultrasonic pulse an object’s dis¬ 
tance can be determined by the time it takes the pulse to travel to and be reflected from the 
object. Again, this technique is highly dependent on line of sight, and distance/free-space 
loss can be a limiting factor. In addition, these sensors allow the identification that an object 
exists, and not who or what the object is. Prior knowledge of what an operator expects to 
see is required to identify objects and determine location. The equipment and processing 
power can also be significant and can be impractical fit onto a small agent. 

The most common technique in use today for outdoor manned and unmanned systems 
is GPS. The only line of sight required is toward the sky, and can be used day or night and 
in a variety weather conditions. Receivers collect unique signals sent from at least four 
satellites. The receiver calculates its distance to each of the satellites by determining the 
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time it took the signal to go from the satellite to the reeeiver, and ultimately triangulate its 
position. However, each signal sent from the satellites is about 300 meters in length, and 
the precision of measuring the signal can vary. Atmospheric effects can also impact the 
measurement of the signal. The ionosphere slows down the signal from the satellite, but 
this delay can also vary by time and location. The imprecise signal measurements and also 
ionospheric disturbances limit the typical accuracy range for standard units to 3-5 meter 
accuracy. 

While this may be good enough for something like driving directions, the desire for close- 
proximity swarm behavior requires a higher degree accuracy. In the scenario put forth in 
this paper, the agents are initially close together (about 0.5 meters) on the USV, so required 
GPS error bars would have to be at least -t/- 0.25 meters. 

An alternative method for providing more accurate GPS is a technique called real-time 
kinematics (RTK). It does not provide absolute position, but will provide a highly accurate 
relative position between two RTK GPS modules. 

In addition to measuring the signal from a GPS satellite, an RTK GPS receiver also mea¬ 
sures the phase of the carrier wave that the signal is being modulated on. This is called 
carrier phase tracking. The carrier wavelength is about 19 centimeters, so the measurement 
of the carrier phase can be much more accurate. However, since the receiver is measuring 
only the phase and ignoring the content of the transmitted signal, it becomes difficult for the 
receiver to align the signals or be off by an integer number of wavelengths. This is called 
integer ambiguity and is usually addressed with statistical methods to reduce the accuracy 
error. 

In addition to the increased accuracy due to carrier phase tracking, and assuming the two 
modules are close enough so that they are receiving the same amount of ionospheric inter¬ 
ference, comparing the carrier phase from one module with the carrier phase received by 
another module can cancel out the adverse effects of the ionosphere on the signal. Sending 
the phase data between the two modules is commonly done by a radio modem in the UHF 
band. Utilizing the differences in the signal received by two GPS units, the relative position 
accuracy between the two units can be resolved to 1-5 centimeters. 
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Also, accurate orientation information is required to effectively control the agent. A num¬ 
ber of options are available to the user for this as well. A simple digital compass would 
give the heading of the agent based off of the Earth’s magnetic field. Inertial Measurement 
Units (IMU) measure the acceleration in six degrees of freedom. For the yaw motion, the 
angular z acceleration is integrated into an angular velocity and the velocity is integrated 
into angular displacement. Given a set initial heading, we can then assume that any angu¬ 
lar displacement in yaw would mean a change in the heading of the agent. Micro-electro 
Mechanical Systems (MEMS) based IMUs have been thoroughly developed and tested and 
are onboard many manned and unmanned systems. 


3.2 COTS Hardware and Open Source Software Design 

This approach will include the open source ROS Indigo software architecture [17] for inte¬ 
gration, COTS Clearpath Robotics Heron USV platform [18], the COTS Parrot AR.Drone 
UAV platform [19], and SwiftNav Piksi GPS [20] for RTK data. We begin with the discus¬ 
sion of software integration first because the choice of communication software drives the 
choices for hardware for this project. 


3.2.1 ROS architecture 

Robot Operating System (ROS) is an open source framework for robot operation develop¬ 
ment initially released in 2007 by Willow Garage. It functions as a publisher-subscriber 
architecture, where one or more machines may publish, or send out, certain information, 
and either the same or other machines may subscribe, or listen, to that information. It pro¬ 
vides a structured communications between machines running different operating systems 
and hardware. It also has online and open source libraries and tools to allow developers 
to quickly utilize previous work in robot applications. ROS packages contain nodes which 
may receive, post and combine sensor, actuator, state and other information. This com¬ 
munications path can be accomplished wirelessly, as long as the agents are on the same 
wireless network. 
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Figure 3.1: Parrot ARDrone Version 2.0 Source: [19]. 


Parrot ARDrone Speeifieations 

Dimensions 

451 X 451 mm 

Proeessor 

IGHz 32 bit ARM Cortex A8 

Total Weight 

380 g 

Max Speed 

21.6 kts (11.11 m/s) 

Payload 

-200 g 

Battery 

1500 mA/H LiPo reehargeable 

Rotor Power Draw 

14.5 W 

Nominal Use Runtime 

15 min 


Table 3.1: Quadrotor Drone Specifications. 


3.2.2 Quadrotor Drone 

The ARDrone 2.0 (Figure 3.1) will be used as the agent experieneing the artifieial spring 
foree. The ARDrone is a quadrotor aireraft from the Freneh eompany Parrot. It was origi¬ 
nally launehed in 2010, with version 2.0 launehing in 2012. 

Through eontrol of the four propellers, the drone ean move with six degrees of freedom: 
forward/baek, vertieal, lateral, roll, piteh and yaw. Battery eapaeity and weight affeet the 
flight time of the ARDrone. In addition to the ARDrone’s 380 g mass, there is also the pro- 
teetive shell, the battery and the user-added external RTK GPS module and antenna whieh 
brings the total mass of the roving agent to 980 g. For the purposes of this experiment, 
the average flight time will be assumed as 15 minutes with a mass of 1 kg (Table 3.1). 
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The drone also eomes with two integrated eameras, one forward faeing and one downward 
faeing (neither of which will be used for this study). 

Internal to the drone are numerous sensors of interest. Pressure sensors measure the ambi¬ 
ent barometric pressure giving an estimation of altitude, a magnetometer that, when cali¬ 
brated, gives the magnetic heading of the drone, and MEMs based IMUs give accelerations 
in 6 degrees of freedom. The angular acceleration is integrated to give angular velocity 
measurements and again integrated to give pitch, roll and yaw angle. Take-off, landing, 
hovering and trimmed flight are all accomplished through the pre-programmed onboard 
processes, the only requirement is the command to do so. Control of the drone is ac¬ 
complished remotely through the onboard 802.1 In ad hoc Wi-Fi network. Normally, one 
would use an iPad or Android device, download the respective applications, connect to the 
ARDrone and begin the flight. 

There are many quadrotor aircraft on the market today, but what makes the ARDrone in¬ 
teresting for this research is the third-party uses. Parrot publicly released the ARDrone 
Software Development Kits (SDK), which allows third-party developers to write their own 
applications for controlling the drone. This will be discussed in the next chapter on system 
integration. 


3.2.3 USV 

The Heron (Figure 3.2) will be used as the USV. It will act as the base station from which 
the agents will determine their relative distance from each other. 



Figure 3.2: Clearpath Robotics Heron USV Source: [18]. 
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Clearpath Robotics Heron Specifications 

Dimensions 

1.35 X 0.98 X 0.32 m 

Total Weight 

30 kg 

Max Speed 

3.3 kts (1.7 m/s) 

Payload 

10 kg 

Battery 

29 a/H NiMH rechargeable 

Thruster Power Draw 

VOW 

Nominal Use Runtime 

2.5 H 


Table 3.2: Surface Vessel Specifications. 


Heron was launched in 2014 from Clearpath Robotics. It is a catamaran design with dif¬ 
ferential thrusters for propulsion and steering. It comes ROS-ready, where onboard sensors 
and actuators are available as ROS topics. Heron can support up to 10 kg of payload (Table 
3.2), more than enough for an ARDrone landing area and base station equipment. Sensors 
onboard include GPS, LiDAR, IMUs, front facing and PTZ (Pan-Tilt-Zoom) camera, tem¬ 
perature, pressure and more. For the purposes of this research, we will only be using the 
onboard GPS sensor to drive a predetermined waypoint course. Control of the USV is 
ac-complished through the onboard 802.11 Wi-Fi network, with a long range wireless 
network station that provides Wi-Fi at distances up to 1km. 


3.2.4 GPS-RTK Module 

The GPS-RTK module that will be used is the Piksi GPS receiver from SwiftNav (Figure 
3.3). The small, light weight, low-power consumption and fast position solution update rate 
make it ideal for use on agents and for this research. Used alone, the Piksi module would 
function as any other GPS receiver, giving standard fix information. However, when 
used in conjunction with another module, the modules will share GPS carrier phase 
information via the 915 MHz radio link. This data is then used to provide relative 
position from one module to the other with centimeter level accuracy. The UHF radio link 
can provide over 1 km of range between the rover agent and the base station. 


SwiftNav provides the software for visualizing the RTK fix positioning between the two 
radio connected modules. In the next chapter, we will discuss how the position information 
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Figure 3.3: SwiftNav Piksi Module Source: [20]. 


SwiftNav Piksi Speeifieations 

Dimensions 

53 X 53 mm 

Proeessor 

168 MHz STM 32F4 ARM 
Cortex-M4 DSP 

Total Weight 

32 g (226 g with GPS 
high gain antenna and UHF 
Telemetry Radio) 

Power Draw 

500 mW 

Supply Voltage 

3.3-5.5 V 

Aeeuraey 

1-5 em (relative) 

Solution Update Rate 

50 Hz 

Table 3.3: RTK GPS 

\/lodule Specifications. 


is sent to the artifieial physies algorithm. 
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CHAPTER 4: 
System Integration 


With the RTK GPS modules and onboard IMUs, we have position and orientation infor¬ 
mation of the agent relative to the base station. We must get the position and orientation 
information of the current state of the agent to the spring-force algorithm to determine 
the required velocity vector for each agent. Once the velocity vector is determined, we 
must then send the respective velocity command to each agent to accomplish the desired 
behavior. 


4.1 Agent Velocity Control and Sensor Data 

To establish a communications path of real-time information and required commands, a 
ROS node must be set up on each agent. Mani Monajjemi from Simon Fraser University, 
released a ROS driver for the Parrot ARDrone [21]. This driver sets up publisher-subscriber 
node, which publishes onboard sensor information (IMU information, battery life, etc.) 
and subscribes to (or listens for) velocity commands sent to the drone. The open source 
software ardrone_autonomy* package allows COTS ARDrones to be set up as a ROS 
node. The ardrone autonomy package is designed for a single instance of the ardrone 
node. Each ARDrone acts as its own wireless router and provides its own ad hoc wireless 
network. One would connect to the ARDrone network via a computer, load the ardrone 
ROS driver, and control (via ROS) one agent from one computer. 


4.2 Multiple Agent Network 

If multiple agents are to be controlled from a single station, more work must be done. NPS 
research assistant Mike Clement [22] put forth an approach to accomplish this. Each of the 
ARDrones are set up via telnet to shut down its local wireless network and connect to a 
separate wireless access point and given a specified IP address. By designating unique IP 
addresses, numerous ARDrones to be connected to the same wireless network. A Python 
script scans the wireless network looking for the pre-set IP addresses for connected agents 

%ttps ://github.com/AutonomyLab/ar dr one_aut onomy 
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and, once a agent is found, load one instance of the ardrone autonomy ROS driver for eaeh 
agent. Unique node and topie names for the agents must also be identified in ROS, so that 
each velocity command generated in the spring algorithm can be sent to the proper agent. 
This allows the system to be able to eontrol (via ROS) multiple agents near simultaneously. 

However, out of the box ARDrones are set up to use the same User Datagram Proto- 
eol (UDP) network ports. So, if two or more agents are used, they eould be trying to 
send their information on the same port, causing interference and dropped packets. Mike 
Clement [22] also set up a UDP remapping tool to resolve this problem by specifying the 
port address that eaeh ARdrone is assigned. Now, practically speaking, we can get the 
heading information of the agent(s) into the artificial physics algorithm and we ean send 
velocity commands to the agents as well. 

4.3 Relative Position Updates 

Still lacking is the position information. The SwiftNav software, when a Piksi module is 
eonneeted, allows the user to see information about the GPS signals reeeived by the mod¬ 
ules. Onee the RTK GPS fix solution has been obtained via the embedded software, user 
can see the North-South (y) and East-West (x) location of one module relative to the other. 
One module is plaeed on the agent as a rover and the other module is designated as a 
base station. But just being able to see the x-y location of the agent does us no good; we 
need to be able to feed that information in near real-time into the artificial physics 
algorithm. A ROS driver, written by Paul Bouchier [23], publishes the x and y 
eoordinates of the RTK fix as a ROS topic and is available open souree on GitHub as the 
swi£tnav_piksi2 driver package. Again, a unique identifier is required for each RTK 
node and topie for position update information. By ehanging the group namespaee in the 
launch file of the package, we ensure that the RTK information is uniquely identified for 
each agent. 

4.4 MATLAB and ROS 

The Robotics Systems Toolbox is an add-on kit for MATLAB that starts a ROS node and 
ean publish and subseribe to topics from a MATLAB script. After initializing the node, 
the spring foree algorithm subseribes to the RTK position information, applies the virtual 

%ttps://github.com/PaulBouchier/swiftnav_piksi 
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spring force, and publishes the required veloeity command to the motor eontroller of each 
agent. 


4.5 Experimental Setup 

This system is set up so that the base station, which consists of a computer and a Piksi 
module, is independent and mobile (Figure 4.1). 


Comms Path CONORS 



RTK Module 



Artificial Physics Algorithm Send velocity command 

via WiFi 



ROS Master 


Figure 4.1: Concept of Operations for Communication Path. 


For the seenario put forth in this paper, the base station is put on Clearpath Roboties Heron 
USV, to allow take off, following and landing multiple agents on a USV. The Heron is 
eontrolled through a separate ROS node. A simple box perimeter following routine is setup 
for the Heron to accomplish while the agents aeeomplish the swarm behavior at the same 
time. 
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CHAPTER 5: 
Experimental Results 


With a tested spring foree algorithm eomputer model, funetioning hardware, and a reli¬ 
able mobile network, we are ready to test the behavior of real-world agents in an outdoor 
environment. For the first three tests, a single agent is made to move in one dimension 
at a loeation relative to the base station. The following “bird’s eye view” diagrams are a 
MATLAB graphieal representation (Figure 5.1) of relative longitude in meters (x-axis) and 
relative latitude in meters (y-axis) as refereneed from the base station loeated at the origin. 
For example, a value of x = 5 would eorrelate to 5 meters East of the base station, while a 
value of X = -5 would eorrelate to 5 meters West of the base station. The rover (designated 
as the green dot) moves along the -l-y axis (i.e., North-South axis) only. The base station 
(designated as the red dot) is set at the origin. 


10 
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-10 -5 0 5 10 

Figure 5.1: Roving Agent (Green) and Base Station (Red). 



The position information of the roving agent is sent to the spring foree algorithm, and the 
output is the veloeity eommand required for the agent to maintain the desired separation 
distanee. For all the following tests, the sping eonstant k is set to 2 N/m and the damping 
eoeffieient is set to 2.8 kg/s, making the system eritieally damped. 
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5.1 Test 1: Validation of Spring Force Algorithm with 
RTK Input {6 = 0, Desired Separation = 5m) 

For the first test (Figure 5.2), the agent is loeated North and faees North, with the y-axis 
of the plot is designated as the North-South axis. Any output veloeity eommand from the 
algorithm should be a +!- y veloeity (i.e., the agent is loeated north of the base station and 
is faeing north; any movement should be simply forward or baekward). Note: The agent is 
not moving freely, the author is walking the agent North and South to eheek the response 
of the system. 




Figure 5.2: Position of Agent and Base Station [6 = 0). 


At the farthest distanee away (about 9 meters), the artifieial physies algorithm ealeulates a 
foree of 8 N faeing toward the base station, pulling it in closer (Figure 5.3). 



Figure 5.3: Resulting Artificial Force on the Agent [6 = 0) at Farthest 
Distance. 


At its closest distance to the base station (about 3 meters), the artificial physics algorithm 
calculates a force of 4 N facing away from the base station, pushing it farther away (Figure 
5.4). 
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Figure 5.4: Resulting Artificial Force on the Agent {6 = 0) at Closest Dis¬ 
tance. 


The resultant foree is integrated using teehniques deseribed in Chapter 2. The output is a 
veloeity eommand, whieh is then sent to the agent (Figure 5.5). 



' 0 2 4 6 0 10 12 14 16 18 20 

nma t [s] 


Figure 5.5: Position of Agent and Resulting Velocity Command {6 = 0). 


We ean see that if the agent position is eloser than desired separation of 5 meters, the y 
veloeity eommand is positive (i.e., if the agent is too elose, then move forward). If the 
agent is farther than the desired separation, then the y veloeity eommand is negative (i.e., 
if the agent is too far away, then move baekward). There is minor movement in the x axis, 
but that is a result of ambient environmental faetors and should be disregarded. 


29 




















5.2 Test 2: Validation of Spring Force Algorithm with Ro¬ 
tational Input (6 = 90, Desired Separation = 5m) 

For the next test (Figure 5.6), the agent was again made to move on the +y axis relative to 
the base station (i.e., North), but this time the agent would be facing 90 degrees counter¬ 
clockwise (i.e., due East). Again, the agent is not moving freely, the author initially 
rotated the agent and then walks the agent North and South to check the response of the 
system. 


Figure 5.6: Position of Agent and Base Station {6 = 90). 


As before, at the farthest distance away, the artificial physics algorithm calculates the same 
force of 8 N facing toward the base station (Figure 5.7). 




Figure 5.7: Resulting Artificial Force on the Agent {6 = 90) at Farthest 
Distance. 


Results are similar as the previous experiment for the closest approach of the agent to the 
base station (Figure 5.8). 

Since the agent has rotated 90 degrees, the resulting velocity command (Figure 5.9) should 
now be given (as a result of the rotation transformation matrix) as a +/- x velocity (i.e., any 
movement should be right or left, since the reference frame of the agent has changed). 

We can see that if the agent position is closer than desired separation of 5 meters, the x 
velocity command is negative (i.e., if the agent is too close, then move left). If the agent 


30 





Figure 5.8: Resulting Artificial Force on the Agent (0 = 90) at Closest 
Distance. 



Tima t [a] 



Figure 5.9: Position of Agent and Resulting Velocity Command {6 = 90). 


is farther than the desired separation, then the x velocity command is positive (i.e., if the 
agent is too far away, then move right). There is minor movement in the x axis during the 
first few seconds because of the agent turning from facing North to facing East to start the 
test. 
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5.3 Test 3: Validation of Computer Model from real- 
world Dynamics 


For the next test, we want to validate that our eomputer model aeeurately refleets the real- 
world dynamics of the quadrotor aircraft. To do that, we input a step velocity command 
in the vertical direction to the computer model and to the actual agent (Figure 5.10). The 
-l-y axis of the 2D plot is now designated as the vertical height above ground, and the agent 
moves only up and down. The ultrasonic altimeter on the agent gives the height above 
ground position of the agent in mm. 



Time I [s] 

livutVeleeity vs. Time 



Figure 5.10: System Response Velocity Step Function. 


Because the real-world system must deal with physical inertia of spinning up the rotors and 
also the time latency of the ROS command, an obvious delay is seen between the computer 
model and the real-world response. To compensate, we add an “inertia” term, a first order 
transfer function in the Laplace domain with a time constant r = 0.2 (Figure 5.11). 

The original computer model had as high as a 25% error between the predicted value and 
the actual value. With the inertia term added, we can see that the error went only to around 
5%. With the inertia term, we can have good confidence that the computer model will 
closely resemble the real-world dynamics of the system. 


5.4 Test 4: Validation of Critically Damped System 


Now, we allow free movement of the agent to test our previous prediction of a critically 
damped system where k = 2 N/m and /3 = 2.8 kg/s. We put the spring in the vertical 
direction with the reference point or base station as the ground. The -l-y axis of the 2D plot 
remains designated as the vertical height above ground, and again the agent only moves up 


32 













% Error vs. Time 



Figure 5.11: Error between Computer Model and real-world Response. 


and down. Desired separation is set to a height of 1.5 m after an initial hovering altitude of 
0.75 m. The computer model predicts the behavior of the system (Figure 5.12). 


Drone Position Reiative to Ground vs. Time 



Figure 5.12: Simulation of Vertical Spring (Critically Damped). 


The simulated agent rise time to the desired distance of 1.5 m was about 0.75 seconds. 

For the real-world test, the ultrasonic altimeter sensor onboard the agent sends its altitude 
position information to the spring algorithm, and the resultant velocity is sent as a command 
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to the agent (Figure 5.13). 


Drone Position Reiative to Ground vs. Time 



Time t [s] 

Figure 5.13: Actual System Response to Vertical Spring (Critically Damped). 


The actual agent rise time to the desired distance of 1.5 m was also about 0.75 seconds. 
The upper limit of the velocity command sent to the agent is set to 0.5 m/s so as to not 
burn out motors or crash into a overhead light. We can also see very little overshoot due 
to the critical damping of the system. Some minor perturbations to the system can be seen 
after reaching steady state due to environmental effects, but similar perturbations are very 
common in real-world operations. 

The agreement between the computer model and the real-world behavior gives credibil¬ 
ity to the system architecture and any future tests. 


5.5 Platform Substitution 

Since the GPS information from a single RTK module must be sent over a UHF radio 
frequency, the amount of data traffic on that frequency increases when trying to scale up 
the number of units. This causes major processing problems with the system, since it is 
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trying to resolve sinee unique GPS data from separate units. The solution is to ehange the 
eomputer proeessing the RTK information from the base station to the agent. This allows 
the base station to broadcast its own location, and then the agents determine their relative 
location from that signal. However, even a small microcomputer to accomplish the RTK 
processing, in addition to the Piksi module and high gain antenna, proved to be too heavy 
for flying ARDrone. We made the decision to substitute the ARDrone quadrotor aircraft 
with a four-wheeled ground robot called Pioneer 3-AT (Figure 5.14). 



Figure 5.14: Pioneer 3-AT. 


The Pioneer is controlled by the same ROS publisher message, so the same script used for 
the previous experiments with the ARDrone can be used for the Pioneer. However, the 
quadcopter accepts velocity commands in six degrees of freedom, as mentioned before. 
The ground robot only accepts velocity commands in two degrees of freedom: forward- 
backward motion and angular z (yaw) motion. This substitution gives many more opportu¬ 
nities to test, while still validating the intent of close cooperative swarm engagements. 
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5.6 Test 5: Multi-agent Robotic Swarm Following 

For the final test of this study, we perform a full demonstration of eontrolling multiple 
robots using an artifieial spring with RTK GPS as the positional input information. 

We start with three pioneer robots (eonveniently named Robot 1, Robot2 and RobotS) about 
5 meters North of the base station, faeing North (Figure 5.15). The “bird’s eye view” di¬ 
agram is still valid, with eaeh of the robots’ position loeated relative to the base station at 
the origin. 



Figure 5.15: Multi-agent Test at Time=0 Seconds. 


The y direetion of the artifieial spring is set to zero. This eauses the robots to feel an 
attraetive foree in the y direetion toward the base station at all times (Figure 5.16). 

The X direetion of the spring is ignored beeause of the ground robot does not have the six 
degrees of freedom as stated before. Although one does not have to ignore the x direetion, 
but to aeeomplish a movement in East-West direetion the robot would have to pivot to faee 
that direetion and then move. For simplieity’s sake and proof of eoneept purposes, we sim¬ 
ply do not send the x eommand to eaeh respeetive robot. 

The robots move toward the stationary base station until the dilferenee in the y-position 
is (elose to) zero (Figure 5.17). 

The base station then moves North and South at a slow walking paee. As the base station 
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Figure 5.16: Multi-agent at Time=l Second. 



Figure 5.17: Multi-agent Test at Time=8 Seconds. 


moves North (in the +y direetion), the robots also move toward the North in order to 
maintain as elose to a zero difFerenee in the y-position as possible (Figure 5.18) 

The same ean be seen as the base station moves South (in the -y direetion) (Figure 5.18). 

The full raw data plot of position information and velocity commands for each of the re¬ 
spective robots can be seen in Figure 5.19. 

This proves that a multi-agent robotic swarm system can be controlled using an artificial 
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Figure 5.18: Multi-agent Test at Time=12 Seconds. 



Figure 5.19: Multi-agent Test at Time=12 Seconds. 


spring algorithm with RTK GPS position inputs. 
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Figure 5.20: Position and Velocity vs. Time (Three Agents, Spring Length 
= 0 ). 
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CHAPTER 6: 
Conclusion 


6.1 Sensor Issues 

Although the RTK GPS information is very useful for precise robotic control, the two mod¬ 
ules need to be receiving signals from numerous GPS satellites. In practice, this means the 
modules need a clear view of the sky. If the robot goes under a tree, or too close to a 
tall building which obstructs a large portion of the sky, the RTK GPS fix information 
be-comes unreliable. Sensors such as GPS, LIDAR, IMUs and others must be evaluated 
for their strengths and weaknesses and then combined in order to provide a system with a 
high degree of accuracy and reliability. 

The IMUs on the ARDrone did not provide the necessary reliability for a robust system. 
The angle of orientation from the onboard IMU jumps around drastically during takeoff, 
most likely due to the spike in electromagnetic interference from power sent to the brush¬ 
less motors. In addition, the angle would process on the order of 10 degrees per minute, 
but that too would vary. Some compensation was added to the script, but determining ori¬ 
entation would be much easier with a better IMU. 

Yet another issue with the IMU is the fact that the ARDrone must be pointing East upon 
start up in order for the x-axis of the RTK GPS fix and the x-axis of the agent to line up. 
Otherwise, the algorithm will, for example, send a North command to the agent, but the 
agent will interpret it as a command to fly to the West. 

The obvious fix would be to use a compass to determine orientation. The ARDrone does 
have a magnetometer onboard, which would solve the IMU problem by providing an abso¬ 
lute heading. However, when publishing on a ROS topic, the magnetometer gets stuck at 
a certain heading and becomes unreliable. A better magnetometer, or a different platform, 
would go a long way in alleviating these headaches in the future. 

Because of the issues with the ARDrone sensors as well as the stability problems because 
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of the additional weight, we did not have the confidence to attempt waterborne formation 
maneuvers. However, the approach outlined can be applied to waterborne operations, with 
better platforms and sensors. 

6.2 Future Work 

A quadrotor platform with a higher payload capacity and more reliable IMUs/magnetometers 
could in theory accomplish the stated goal of having multiple UAVs launch from a USV 
and executing a task using the method put forth in this research. In addition, the launch and 
recovery of fixed-wing autonomous agents from a USV would offer much greater range, 
flight time and payload capacity than a quadrotor aircraft. The techniques described in this 
paper can also easily be applied to fixed-wing aircraft, with the obvious understanding that 
they do not have the ability to hover. 

With the IMU data, the RTK GPS data, and our predictive algorithms, a natural progression 
would be the introduction of a Kalman filter. This would give better confidence of the true 
position of the agent even if sensor data from one source is degraded, as the RTK GPS data 
would be if traveling under tree cover. Additional sensors such as LIDAR and SONAR can 
also be included to detect obstacles that are obviously not broadcasting their GPS location 
on our ROS network. 

The Navy can greatly benefit from the use of unmanned systems accomplishing increasing 
roles and responsibilities in mission related tasks. Algorithms that introduce a high level of 
autonomy relieve the burden of direct control of a robot, allowing for the use of numerous 
agents and allowing the operator to focus on big picture strategic issues. 


42 



APPENDIX: MATLAB CODE 


MATLAB script for Artificial Spring with three agents using ROS toolkit (with RTK posi¬ 
tion publishers and velocity command subscribers). 

rosinit 

function [ ForceX, ForceY ] = hForce2D( xl, yl, x2, y2, rO, k) 
distR = sqrt((xl-x2)^2 -i- (yl-y2)^2); 
forccTemp = -k*(distR-rO); 

ForceX = forceTemp*(xl-x2)/distR; 

ForceY = forceTemp*(yl-y2)/distR; 
end 

function rtkl(~, message) 

% Declare global variables to store position and orientation 
global xl 
global yl 
global zl 

% Extract position and orientation from the ROS message and assign the 
% data to the global variables, 
xl = [message.Pose.Pose.Position.X]; 
yl = [message.Pose.Pose.Position.Y]; 
zl = [message.Pose.Pose.Position.Z]; 
end 

function rtk2(~, message) 
global x2 
global y2 
global z2 

x2 = [message.Pose.Pose.Position.X]; 
yl - [message.Pose.Pose.Position.Y]; 
z2 = [message.Pose.Pose.Position.Z]; 
end 
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function rtk3(~, message) 
global x3 
global y3 
global z3 

x3 = [message.Pose.Pose.Position.X]; 
y3 = [message.Pose.Pose.Position.Y]; 
z3 = [message.Pose.Pose.Position.Z]; 
end 

swarm_size = 3; 

mass = 1; %mass of one drone 

t_s = 0.1; %time step size in seconds 

iter = 10000; %number of time steps 

beta = 2.8; %damping constant 

k = 2; %spring constant of agents 

kl = -2; %spring constant of base station 

rO = 7; %separation from other agents 

rl = 1; %separation from base station 

% initialize starting velocity and acceleration 

[dampForceX,dampForceY,X_vel,X_vel_xfrm,X_accel,Y_vel,Y_vel_xfrm,Y_accel, 
x,y, theta] = deal(zeros(iter,swarm_size)); 

% Create a subscriber for the RTK fix topic 
rtkfixl = rossubscriber(7gps/rtkfix’, @rtkl); 
rtkfix2 = rossubscriber(7gps_2/rtkfix’, @rtk2); 
rtkfix3 = rossubscriber(7gps_3/rtkfix’, @rtk3); 
global xl x2 x3 yl y2 y3 

% Create a publisher for the ROS TWIST message 
publisher = rospublisher(7robotl/cmd_vel’, ’geometry_msgs/Twist’); 
twist = rosmessage(rostype.geometry_msgs_Twist); 

publisher2 = rospublisher(7robot2/cmd_vel’, ’geometry_msgs/Twist’); 
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twist2 = rosmessage(rostype.geometry_msgs_Twist); 

publishers = rospublisher(7robot3/cmd_ver, ’geometry_msgs/Twist’); 
twists = rosmessage(rostype.geometry_msgs_Twist); 

[twist.Linear.X,twist. Angular.Z,twist2.Linear.X,twist2.Angular.Z, 
twists.Linear.X, twistS.Angular.Z] = deal(O); 

send(publisher, twist); 
send(publisher2,twist2); 
send(publisherS ,twistS); 

X_pos_OP = 0; %base station at origin 
Y_pos_OP = 0; 

pause(l) 

% % Main Loop 
for j =2 liter 
for i=l:swarm_size 
TotForeeX^O; 

TotForeeY^O; 

% get rtk info in X-Y (East - North) eoordinates 
x(j,l) ^ xl; 

y(j,i)-yi; 


x(j,2) ^ x2; 
y(j,2) - y2; 


x(j,S) ^ xS; 

y(j,S) ^ yS; 

for index=l:swarm_size 
if index~=i 
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[ForceX, ForceY] = hForce2D(x(j,i),y(j>i)> x(j,index), y(j,index), 
rO, k); 

[OP_ForceX, OP_ForceY] = hForce2D(X_pos_OP, Y_pos_OP, 
x(j,index), y(j,index), rl, kl); 

TotForceX= TotForceX + ForceX + OP_ForceX; 

TotForceY= TotForceY + Force Y + OP_ForceY; 
end 
end 

dampForceX(j,i) = TotForceX - beta.*X_vel(j-l,i); 
dampForceY(j,i) = TotForceY - beta.*Y_vel(j-l,i); 

X_accel(j,i) = dampForceX(j,i)/mass; 

Y_accel(j,i) = dampForceY(j,i)/mass; 

X_vel(j,i) = X_vel(j-l,i) + 0.5.*(X_accel(j-l,i)+X_accel(j,i)).*t_s 
Y_vel(j,i) = Y_vel(j-l,i) + 0.5.*(Y_accel(j-l,i)+Y_accel(j,i)).*t_s 

%reference frame transformation 
X_vel_xfrm(j,i) = (X_vel(j,i).*cosd(theta(j,i)) 

+ Y_vel(j,i).*sind(theta(j,i))); 
Y_vel_xfrm(j,i) = (-X_vel(j,i).*sind(theta(j,i)) 

+ Y_vel(j,i).*cosd(theta(j,i))); 

twist.Linear.X= X_vel_xfrm(j,l); 
twist2.Linear.X=X_vel_xfrm(j ,2); 
twists.Linear.X=X_vel_xfrm(j,3); 
twist.Linear.Y= Y_vel_xfrm(j,l); 
twist2.Linear. Y=Y_vel_xfrm(j ,2); 
twists.Linear. Y=Y_vel_xfrm(j,3); 

send(publisher,twist); 
send(publisher2,twist2); 
send(publisher3,twist3); 
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clf; 

h=plot(x(j,l),y(j,l), ’o’); 

set(h,’ MarkerEdgeColor ’,’ none’,’ MarkerFaceColor’,’ g ’) 
hold on 

h2=plot(x(j,2),y(j,2), ’o’); 

set(h2,’MarkerEdgeColor ’,’ none’,’ MarkerEaeeColor ’,’ g’) 
h3=plot(x(j,3),y(j,3), ’o’); 

set(h3,’ MarkerEdgeColor’,’ none’,’ MarkerEaeeColor ’,’ g ’) 
hl=plot(X_pos_OP, Y_pos_OP, ’o’); 
set(h 1, ’ MarkerEdgeColor’,’ none ’ ,’MarkerEaeeColor’ ,’r’) 
axis([-10 10 -10 10]); 

pause(.l); 

end; 
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